﻿using UnityEngine;
using System;
using System.Collections;
using Kinect2;

public class DepthBasic : MonoBehaviour {

	KinectSensor kinectSensor;
	DepthFrameReader depthFrameReader;
	FrameDescription depthFrameDescription;
	Texture2D texture;
	GameObject status;
	const int MapDepthToByte = 8000 / 256;

	void Start () 
	{
        kinectSensor = KinectSensor.GetDefault();
			
        depthFrameReader = kinectSensor.DepthFrameSource.OpenReader();
        depthFrameReader.FrameArrived += Reader_FrameArrived;

		depthFrameDescription = kinectSensor.DepthFrameSource.FrameDescription;
        texture = new Texture2D(depthFrameDescription.Width, depthFrameDescription.Height, TextureFormat.BGRA32, false);
            
        kinectSensor.IsAvailableChanged += Sensor_IsAvailableChanged;
        kinectSensor.Open();

        InitializeComponent();
	}
	
	void Update () 
    {
        EventRouter.Check();
	}

	void OnApplicationQuit()
	{
        if (depthFrameReader != null)
            depthFrameReader.Dispose();
        depthFrameReader = null;

		if (kinectSensor != null)
			kinectSensor.Close ();
        kinectSensor = null;
	}

    void InitializeComponent()
    {
        GameObject depth = GameObject.Find("depth");
        depth.guiTexture.texture = texture;
        status = GameObject.Find("status");
        status.guiText.text = kinectSensor.IsAvailable ? "Running" : "No ready Kinect found!";
    }

    void Reader_FrameArrived(object sender, DepthFrameArrivedEventArgs e)
	{
        using (DepthFrame depthFrame = e.FrameReference.AcquireFrame())
        {
            if (depthFrame != null)
            {
                using (KinectBuffer depthBuffer = depthFrame.LockImageBuffer())
                {
                    if (((depthFrameDescription.Width * depthFrameDescription.Height) == (depthBuffer.Size / depthFrameDescription.BytesPerPixel))
                        && (depthFrameDescription.Width == texture.width)
                        && (depthFrameDescription.Height == texture.height))
                    {
                        ushort maxDepth = ushort.MaxValue;
                        ProcessDepth(depthBuffer.UnderlyingBuffer, depthFrame.DepthMinReliableDistance, maxDepth);
                    }
                }
            }
        }
    }
    
    void Sensor_IsAvailableChanged(object sender, IsAvailableChangedEventArgs e)
    {
        status.guiText.text = kinectSensor.IsAvailable ? "Running" : "Kinect not available!";
    }

	unsafe void ProcessDepth(IntPtr depthFrameData, ushort minDepth, ushort maxDepth)
	{
		ushort* data = (ushort*)depthFrameData;
		int width = texture.width;
		int height = texture.height;
		byte[] data2 = new byte[width * height * 4];
		for (int j = 0; j < height; j++){
			int dst = width * 4 * (height - 1 - j);
			for (int i = 0; i < width; i++) {
				ushort d = (*data++);
				byte intensity =(byte)((d >= minDepth) && (d <= maxDepth) ? (d /MapDepthToByte) : 0);
				data2[dst + i * 4] = intensity;
				data2[dst + i * 4 + 1] = intensity;
				data2[dst + i * 4 + 2] = intensity;
				data2[dst + i * 4 + 3] = 0xff;
				
			}
		}
		texture.LoadRawTextureData (data2);
		texture.Apply();
	}
}
